#include "stm32f10x.h"                  // Device header
#include "PWM.h"
#include "Serial.h"

void Servo_Init(void)
{
	PWM_Init();
}

//TIM2
void Servo_SetAngle_TIM2_CH1_PA0(float Angle)
{
	PWM_SetCompare_TIM2_1(Angle / 180 * 2000 + 500);
}

void Servo_SetAngle_TIM2_CH2_PA1(float Angle)
{
	PWM_SetCompare_TIM2_2(Angle / 180 * 2000 + 500);
}

void Servo_SetAngle_TIM2_CH3_PA2(float Angle)
{
	PWM_SetCompare_TIM2_3(Angle / 180 * 2000 + 500);
}


//TIM1
void Servo_SetAngle_TIM1_CH1_PA8(float Angle)
{
	PWM_SetCompare_TIM1_1(Angle / 180 * 2000 + 500);
}

void Servo_SetAngle_TIM1_CH2_PA9(float Angle)
{
	PWM_SetCompare_TIM1_2(Angle / 180 * 2000 + 500);
}

void Servo_SetAngle_TIM1_CH4_PA11(float Angle)
{
	PWM_SetCompare_TIM1_4(Angle / 180 * 2000 + 500);
}


//TIM3
void Servo_SetAngle_TIM3_CH1_PA6(float Angle)
{
	PWM_SetCompare_TIM3_1(Angle / 180 * 2000 + 500);
}

void Servo_SetAngle_TIM3_CH2_PA7(float Angle)
{
	PWM_SetCompare_TIM3_2(Angle / 180 * 2000 + 500);
}

void Servo_SetAngle_TIM3_CH4_PB1(float Angle)
{
	PWM_SetCompare_TIM3_4(Angle / 180 * 2000 + 500);
}



//TIM4
//void Servo_SetAngle_TIM4_CH1_PB6(float Angle)
//{
//	PWM_SetCompare_TIM4_1(Angle / 180 * 2000 + 500);
//}

//void Servo_SetAngle_TIM4_CH4_PB9(float Angle)
//{
//	PWM_SetCompare_TIM4_4(Angle / 180 * 2000 + 500);
//}

void Servo_Control_Left(void)
{
	if( Serial_Rx_Data[0] == 0x01)
	{
		Servo_SetAngle_TIM2_CH1_PA0(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x02)
	{
		Servo_SetAngle_TIM2_CH2_PA1(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x03)
	{
		Servo_SetAngle_TIM2_CH3_PA2(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x04)
	{
		Servo_SetAngle_TIM3_CH1_PA6(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x05)
	{
		Servo_SetAngle_TIM3_CH2_PA7(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x06)
	{
		Servo_SetAngle_TIM3_CH4_PB1(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x07)
	{
		Servo_SetAngle_TIM1_CH1_PA8(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x08)
	{
		Servo_SetAngle_TIM1_CH2_PA9(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x09)
	{
		Servo_SetAngle_TIM1_CH4_PA11(Serial_Rx_Data[1]);
	}
}

void Servo_Control_Right(void)
{
	if( Serial_Rx_Data[0] == 0x91)
	{
		Servo_SetAngle_TIM2_CH1_PA0(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x92)
	{
		Servo_SetAngle_TIM2_CH2_PA1(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x93)
	{
		Servo_SetAngle_TIM2_CH3_PA2(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x94)
	{
		Servo_SetAngle_TIM3_CH1_PA6(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x95)
	{
		Servo_SetAngle_TIM3_CH2_PA7(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x96)
	{
		Servo_SetAngle_TIM3_CH4_PB1(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x97)
	{
		Servo_SetAngle_TIM1_CH1_PA8(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x98)
	{
		Servo_SetAngle_TIM1_CH2_PA9(Serial_Rx_Data[1]);
	}
	if( Serial_Rx_Data[0] == 0x99)
	{
		Servo_SetAngle_TIM1_CH4_PA11(Serial_Rx_Data[1]);
	}
}



